/***************************************************************************
 创建者: 华磊
 开始时间: 2018.9.26
 copyright: (C) 华友高新
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) 李文友 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "linematch.h"
#include <qdebug.h>


using namespace std;

LineMatch::LineMatch()
{

}

int LineMatch::showDebugWindow(int id, const Mat &imageIn)
{
    if(true==safeMutex.tryLock())
    {
        displayLine(imageIn,detectedLines);
        displayVMark(imageIn, x_detected, y_detected, r_detected);
        safeMutex.unlock();
    }
    return 1;
}

int LineMatch::setLineDetectPara(double rhoIn, double thetaIn, int thresholdIn, double minLineLengthIn, double maxLineGapIn)
{
    rho=rhoIn;
    theta=thetaIn;
    threshold_pt=thresholdIn;
    minLineLength=minLineLengthIn;
    maxLineGap=maxLineGapIn;
    return 1;
}



bool LineMatch::getCrossPoint(Vec4i firstLine, Vec4i secondLine,Point2f &crossPoint,
                              double &crossDistance1,double &crossDistance2,double &angle_a, double &angle_b)
{
    double ka,kb;
    ka = (double)(firstLine[3] - firstLine[1]) / (double)(firstLine[2] - firstLine[0]); //求出LineA斜率
    kb = (double)(secondLine[3] - secondLine[1]) / (double)(secondLine[2] - secondLine[0]); //求出LineB斜率

    if(fabs(ka - kb)<0.000001)
    {
        return false;
    }

    crossPoint.x = (ka*firstLine[0] - firstLine[1] - kb*secondLine[0] + secondLine[1]) / (ka - kb);
    crossPoint.y = (ka*kb*(firstLine[0] - secondLine[0]) + ka*secondLine[1] - kb*firstLine[1]) / (ka - kb);

    //离交点最近的为起始点
    vector<double> tmpDistance;
    tmpDistance.resize(4);
    tmpDistance[0]=sqrt((crossPoint.x-firstLine[0])*(crossPoint.x-firstLine[0])+(crossPoint.y-firstLine[1])*(crossPoint.y-firstLine[1]));
    tmpDistance[1]=sqrt((crossPoint.x-firstLine[2])*(crossPoint.x-firstLine[2])+(crossPoint.y-firstLine[3])*(crossPoint.y-firstLine[3]));
    tmpDistance[2]=sqrt((crossPoint.x-secondLine[0])*(crossPoint.x-secondLine[0])+(crossPoint.y-secondLine[1])*(crossPoint.y-secondLine[1]));
    tmpDistance[3]=sqrt((crossPoint.x-secondLine[2])*(crossPoint.x-secondLine[2])+(crossPoint.y-secondLine[3])*(crossPoint.y-secondLine[3]));

    if(tmpDistance[0]>tmpDistance[1])
    {
        crossDistance1=tmpDistance[1];
        angle_a=atan2(firstLine[1]-crossPoint.y,firstLine[0]-crossPoint.x);
    }
    else
    {
        crossDistance1=tmpDistance[0];
        angle_a=atan2(firstLine[3]-crossPoint.y,firstLine[2]-crossPoint.x);
    }

    if(tmpDistance[2]>tmpDistance[3])
    {
        crossDistance2=tmpDistance[3];
        angle_b=atan2(secondLine[1]-crossPoint.y,secondLine[0]-crossPoint.x);
    }
    else
    {
        crossDistance2=tmpDistance[2];
        angle_b=atan2(secondLine[3]-crossPoint.y,secondLine[2]-crossPoint.x);
    }


    return true;
}

bool LineMatch::getCrossPoint_double(vector<double> firstLine, vector<double> secondLine, Point2f &crossPoint, double &crossDistance1, double &crossDistance2, double &angle_a, double &angle_b)
{
    double ka,kb;
    ka = (double)(firstLine[3] - firstLine[1]) / (double)(firstLine[2] - firstLine[0]); //求出LineA斜率
    kb = (double)(secondLine[3] - secondLine[1]) / (double)(secondLine[2] - secondLine[0]); //求出LineB斜率

    if(fabs(ka - kb)<0.000001)
    {
        return false;
    }

    crossPoint.x = (ka*firstLine[0] - firstLine[1] - kb*secondLine[0] + secondLine[1]) / (ka - kb);
    crossPoint.y = (ka*kb*(firstLine[0] - secondLine[0]) + ka*secondLine[1] - kb*firstLine[1]) / (ka - kb);

    //离交点最近的为起始点
    vector<double> tmpDistance;
    tmpDistance.resize(4);
    tmpDistance[0]=sqrt((crossPoint.x-firstLine[0])*(crossPoint.x-firstLine[0])+(crossPoint.y-firstLine[1])*(crossPoint.y-firstLine[1]));
    tmpDistance[1]=sqrt((crossPoint.x-firstLine[2])*(crossPoint.x-firstLine[2])+(crossPoint.y-firstLine[3])*(crossPoint.y-firstLine[3]));
    tmpDistance[2]=sqrt((crossPoint.x-secondLine[0])*(crossPoint.x-secondLine[0])+(crossPoint.y-secondLine[1])*(crossPoint.y-secondLine[1]));
    tmpDistance[3]=sqrt((crossPoint.x-secondLine[2])*(crossPoint.x-secondLine[2])+(crossPoint.y-secondLine[3])*(crossPoint.y-secondLine[3]));

    if(tmpDistance[0]>tmpDistance[1])
    {
        crossDistance1=tmpDistance[1];
        angle_a=atan2(firstLine[1]-crossPoint.y,firstLine[0]-crossPoint.x);
    }
    else
    {
        crossDistance1=tmpDistance[0];
        angle_a=atan2(firstLine[3]-crossPoint.y,firstLine[2]-crossPoint.x);
    }

    if(tmpDistance[2]>tmpDistance[3])
    {
        crossDistance2=tmpDistance[3];
        angle_b=atan2(secondLine[1]-crossPoint.y,secondLine[0]-crossPoint.x);
    }
    else
    {
        crossDistance2=tmpDistance[2];
        angle_b=atan2(secondLine[3]-crossPoint.y,secondLine[2]-crossPoint.x);
    }


    return true;
}

//void drawDetectedLines(cv::Mat& image, cv::Scalar color = cv::Scalar(255, 255, 255))
//{
//    std::vector&lt;cv::Vec4i&gt;::const_iterator it = lines.begin();
//    while (it!=lines.end())
//    {
//        cv::Point pt1((*it)[0], (*it)[1]);
//        cv::Point pt2((*it)[2], (*it)[3]);
//        cv::line(image, pt1, pt2, color);
//        it++;
//    }
//}

int LineMatch::detectLine(QString filePath)
{
    Mat src,transformedPicture;
//    src = imread("./1.pgm");
    src = imread(filePath.toStdString());
    processPicture(src,transformedPicture);
//    Canny(src, src, 100, 200, 3);
    vector<Vec4i> linesOut;
    detectLine(transformedPicture,linesOut );
    displayLine(src,linesOut);

    return linesOut.size();
}

int LineMatch::detectVMark_mapFile(QString filePath,double min_angle, double max_angle, double max_distance,
                                   double allow_vDistance, double allow_vAngle,
                                   vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{

    Mat src,transformedPicture;
//    src = imread("./1.pgm");
    src = imread(filePath.toStdString());
    processPicture(src,transformedPicture);

    detectVMark_mat(transformedPicture,min_angle,max_angle,max_distance,allow_vDistance,allow_vAngle,x_out,y_out,r_out);

    return 1;

}

int LineMatch::detectVMark_mat(const Mat &imageIn, double min_angle, double max_angle, double max_distance,
                       double allow_vDistance, double allow_vAngle, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    /*检查图像是否载入*/
    if (imageIn.empty())
    {
        printf("Error , LineMatch::detectVMark_mat...imageIn is empty\n");
        return -1;
    }
    safeMutex.lock();
    vector<Vec4i> linesOut;
    detectLine(imageIn,linesOut );
//    displayLine(imageIn,linesOut);

    detectVMark(linesOut, min_angle, max_angle, max_distance, x_out, y_out, r_out);
    deleteTheSameVMark(allow_vDistance,allow_vAngle,x_out, y_out, r_out);
//    displayVMark(imageIn, x_out, y_out, r_out);

    //for debug
    detectedLines=linesOut;
    x_detected=x_out;
    y_detected=y_out;
    r_detected=r_out;
    safeMutex.unlock();
    return 1;
}

int LineMatch::processPicture(const Mat &originPicture, Mat &transformedPicture)
{
    Mat grayImg, binImg;
    /*检查图像是否载入*/
    if (originPicture.empty()) {
        printf("Error Loading Image...\n");
        return -1;
    }

    /*转为灰度图*/
    if (originPicture.channels() == 3)
    {
        cvtColor(originPicture, grayImg, CV_BGR2GRAY);
    }
    else if (originPicture.channels() == 2)
    {
        grayImg = originPicture.clone();
    }
//    imshow("灰度图 image", grayImg);

    /*二值化*/
    threshold(grayImg, binImg, 100, 255, THRESH_BINARY);
//    //adaptiveThreshold(grayImg, binImg, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, 15, -2);
//    imshow("binary image", binImg);


    /*腐蚀*/
    Mat element = getStructuringElement(MORPH_RECT, Size(2, 1));
    Mat erodeImg;
    erode(binImg, erodeImg, element);
//    imshow("erode", erodeImg);

    transformedPicture=~erodeImg;
    //transformedPicture=erodeImg;
    imshow("blackBackGround", transformedPicture);
    return 1;
}

int LineMatch::detectLine( const Mat &imagineSource, vector<Vec4i> &linesOut)
{
    /*霍夫直线检测*/
 //   HoughLinesP(imagineSource, linesOut, rho, theta, threshold_pt, minLineLength, maxLineGap);
    HoughLines(imagineSource, linesOut, rho, theta, threshold_pt);

//    CV_EXPORTS_W void HoughLines( InputArray image, OutputArray lines,
//                                  double rho, double theta, int threshold,
//                                  double srn=0, double stn=0 );
//    CV_EXPORTS_W void HoughLinesP( InputArray image, OutputArray lines,
//                                   double rho, double theta, int threshold,
//                                   double minLineLength=0, double maxLineGap=0 );
}

int LineMatch::detectVMark(const vector<Vec4i> &linesIn, double min_angle, double max_angle,double max_distance,
                           vector<double> &x_out,vector<double> &y_out,vector<double> &r_out)
{
    x_out.clear();
    y_out.clear();
    r_out.clear();
    double tmpX,tmpY,tmpR;
    for(int i=0;i<linesIn.size()-1;i++)
    {
        for(int j=i+1;j<linesIn.size();j++)
        {
            bool isOk=isVMark(linesIn[i],linesIn[j],min_angle,max_angle,max_distance,tmpX,tmpY,tmpR);
            if(true==isOk)
            {
                x_out.push_back(tmpX);
                y_out.push_back(tmpY);
                r_out.push_back(tmpR);
            }
        }

    }
    return 1;
}

bool LineMatch::isVMark(Vec4i firstLine, Vec4i secondLine, double min_angle, double max_angle,
                        double max_distance, double &x_out,double &y_out, double &r_out)
{
    //焦点距离最小值必须小于给定值

    Point2f crossPoint;
    double crossDistance1,crossDistance2;
    double angle_a, angle_b;

    vector<double> tmp_firstLine,tmp_secondLine;
    tmp_firstLine.resize(4);
    tmp_secondLine.resize(4);
    tmp_firstLine[0]=firstLine[0];
    tmp_firstLine[1]=firstLine[1];
    tmp_firstLine[2]=firstLine[2];
    tmp_firstLine[3]=firstLine[3];
    tmp_secondLine[0]=secondLine[0];
    tmp_secondLine[1]=secondLine[1];
    tmp_secondLine[2]=secondLine[2];
    tmp_secondLine[3]=secondLine[3];
    bool isOk=getCrossPoint_double(tmp_firstLine,tmp_secondLine,crossPoint,crossDistance1,crossDistance2,angle_a, angle_b);
    if(false==isOk)
    {
        return false;
    }

    if(crossDistance1>max_distance || crossDistance2>max_distance )
    {
        return false;
    }

    //判断夹角
    double angle=angleRotationJudgement(angle_a,angle_b);
    if(fabs(angle)<min_angle || fabs(angle)>max_angle)
    {
        return false;
    }

    x_out=crossPoint.x;
    y_out=crossPoint.y;
    r_out=angle_a-angle/2;
    return true;

}

int LineMatch::displayVMark(const Mat &originPicture, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    if( originPicture.empty())
    {
        return -1;
    }
    Mat result = originPicture.clone();
    double tmpLength=100;

    for (int i = 0; i < x_out.size(); i++)
    {
        circle(result, Point(x_out[i], y_out[i]), 20,Scalar(150, 255, 0), 2,8);
        line(result, Point(x_out[i], y_out[i]), Point(x_out[i]+tmpLength*cos(r_out[i]), y_out[i]+tmpLength*sin(r_out[i])),
             Scalar(150, 255, 0), 2, 8);
    }

    QString showMsg="vmarkSize=";
    showMsg+=QString::number(x_out.size());
    putText(result,showMsg.toStdString(),Point(10,15),CV_FONT_HERSHEY_SIMPLEX,0.5,Scalar(255,100,0),2,2);

    imshow("displayVMark", result);
    return 1;
}

int LineMatch::displayLine(const Mat &originPicture, vector<Vec4i> &linesIn)
{
    if( originPicture.empty()  )
    {
        return -1;
    }
    Mat result = originPicture.clone();

    for (int i = 0; i < linesIn.size(); i++)
    {
        line(result, Point(linesIn[i][0], linesIn[i][1]), Point(linesIn[i][2], linesIn[i][3]), Scalar(200, 255, 50), 2, 8);
    }
    QString showMsg="lineSize=";
    showMsg+=QString::number(linesIn.size());
    putText(result,showMsg.toStdString(),Point(10,15),CV_FONT_HERSHEY_SIMPLEX,0.5,Scalar(255,100,0),2,2);

    imshow("displayLine", result);
    return 1;
}

int LineMatch::deleteTheSameVMark(double allowDistance, double allowAngle, vector<double> &x_out,
                                  vector<double> &y_out, vector<double> &r_out)
{
    vector<double> tmp_x,tmp_y, tmp_r;
    if(x_out.size()>=1)
    {
        tmp_x.push_back(x_out[0]);
        tmp_y.push_back(y_out[0]);
        tmp_r.push_back(r_out[0]);
    }

    for(int i=0;i<x_out.size();i++)
    {
        int tmpTargetCount=tmp_x.size();
        int isSameCount=0;
        for(int j=0;j<tmpTargetCount;j++)
        {
            bool isOk=isTheSameVMark(allowDistance,allowAngle,x_out[i],y_out[i],r_out[i]
                                     ,tmp_x[j],tmp_y[j],tmp_r[j]);
            if(true==isOk)
            {
                break;
            }
            else
            {
                isSameCount++;
            }
        }
        if(isSameCount==tmpTargetCount)
        {
            tmp_x.push_back(x_out[i]);
            tmp_y.push_back(y_out[i]);
            tmp_r.push_back(r_out[i]);
        }

    }
    x_out=tmp_x;
    y_out=tmp_y;
    r_out=tmp_r;
    return 1;
}

bool LineMatch::isTheSameVMark(double allowDistance, double allowAngle, double x_1, double y_1, double r_1,
                               double x_2, double y_2, double r_2)
{
    double tmpDistance=  sqrt((x_1-x_2)*(x_1-x_2)+(y_1-y_2)*(y_1-y_2));
    double tmpAngle=angleRotationJudgement(r_1,r_2);

    if(tmpDistance<allowDistance &&  fabs(tmpAngle)<allowAngle)
    {
        return true;
    }
    else
    {
        return false;
    }

    return false;

}


double LineMatch::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;

}
